#include <math.h>

#include "spl0601.h"

using namespace NXMFC;

#define SPL0601_I2C_ADDR        0x76

#define PRESSURE_SENSOR         0
#define TEMPERATURE_SENSOR      1

#define CONTINUOUS_PRESSURE     1
#define CONTINUOUS_TEMPERATURE  2
#define CONTINUOUS_P_AND_T      3

spl0601::spl0601()
{
    temp = 0;
    temperature_raw = -1000;
}

spl0601::~spl0601()
{
}

bool spl0601::init()
{
    getCalibParam();

    setRate(PRESSURE_SENSOR, 128, 32);
    setRate(TEMPERATURE_SENSOR, 32, 8);

    writeReg(0x08, CONTINUOUS_P_AND_T + 4);

    return true;
}

bool spl0601::rawPressure(int32_t &raw)
{
    uint8_t buff[3];

    if (readv(SPL0601_I2C_ADDR, 0x00, buff, 3) != 0)
        return false;

    raw = (int32_t)buff[0]<<16 | (int32_t)buff[1]<<8 | (int32_t)buff[2];
    raw = (raw&0x800000) ? (0xFF000000|raw) : raw;

    return true;
}

float spl0601::pressure(void)
{
    float fTsc, fPsc;
    float qua2, qua3;
    float fPCompensate;
    int32_t pressure_raw;

    rawTemperature(temperature_raw);
    rawPressure(pressure_raw);

    fTsc = this->temperature_raw / (float)this->i32kT;
    fPsc = pressure_raw / (float)this->i32kP;
    qua2 = this->c10 + fPsc * (this->c20 + fPsc * this->c30);
    qua3 = fTsc * fPsc * (this->c11 + fPsc * this->c21);
	//qua3 = 0.9f *fTsc * fPsc * (p_spl0601->calib_param.c11 + fPsc * p_spl0601->calib_param.c21);
    fPCompensate = this->c00 + fPsc * qua2 + fTsc * this->c01 + qua3;
	//fPCompensate = p_spl0601->calib_param.c00 + fPsc * qua2 + 0.9f *fTsc  * p_spl0601->calib_param.c01 + qua3;

    return fPCompensate;
}

bool spl0601::rawTemperature(int32_t &raw)
{
    uint8_t buff[3] = {0};

    if (readv(SPL0601_I2C_ADDR, 0x03, buff, 3) != 0)
        return false;

    raw = (int32_t)buff[0]<<16 | (int32_t)buff[1]<<8 | (int32_t)buff[2];
    raw = (raw&0x800000) ? (0xFF000000|raw) : raw;

    temperature_raw = raw;

    return true;
}

float spl0601::temperature(void)
{
    float fTCompensate;
    float fTsc;

    rawTemperature(temperature_raw);

    fTsc = this->temperature_raw / (float)this->i32kT;
    fTCompensate = this->c0 * 0.5 + this->c1 * fTsc;

    return fTCompensate;
}

float spl0601::pressureToAltitude(float p)
{
#define CONST_PF 0.1902630958	//(1/5.25588f) Pressure factor
#define P0   101325.0f			    // standard static pressure at sea level

    if (p > 0)
    {
        return ((::powf((P0 / p), CONST_PF) - 1.0f) * (0 + 273.15f)) / 0.0065f * 100; //in cm
    }
    else
    {
        return 0;
    }
}

bool spl0601::writeReg(uint8_t addr, uint8_t val)
{
    return writeb(0x76, addr, val) == 0;
}

uint8_t spl0601::readReg(uint8_t addr)
{
    uint8_t val = 0;

    readb(0x76, addr, val);

    return val;
}

void spl0601::setRate(uint8_t sensor, uint8_t samplerate, uint8_t oversample)
{
    uint8_t val = 0;
    int32_t i32kPkT = 0;

    switch (samplerate)
    {
    case 2:
        val |= (1 << 4);
        break;
    case 4:
        val |= (2 << 4);
        break;
    case 8:
        val |= (3 << 4);
        break;
    case 16:
        val |= (4 << 4);
        break;
    case 32:
        val |= (5 << 4);
        break;
    case 64:
        val |= (6 << 4);
        break;
    case 128:
        val |= (7 << 4);
        break;
    case 1:
    default:
        break;
    }

    switch (oversample)
    {
    case 2:
        val |= 1;
        i32kPkT = 1572864;
        break;
    case 4:
        val |= 2;
        i32kPkT = 3670016;
        break;
    case 8:
        val |= 3;
        i32kPkT = 7864320;
        break;
    case 16:
        i32kPkT = 253952;
        val |= 4;
        break;
    case 32:
        i32kPkT = 516096;
        val |= 5;
        break;
    case 64:
        i32kPkT = 1040384;
        val |= 6;
        break;
    case 128:
        i32kPkT = 2088960;
        val |= 7;
        break;
    case 1:
    default:
        i32kPkT = 524288;
        break;
    }

    if (sensor == 0)
    {
        this->i32kP = i32kPkT;
        writeReg(0x06, val);
        if (oversample > 8)
        {
            val = readReg(0x09);
            writeReg(0x09, val | 0x04);
        }
    }

    if (sensor == 1)
    {
        this->i32kT = i32kPkT;
        writeReg(0x07, val | 0x80); //Using mems temperature
        if (oversample > 8)
        {
            val = readReg(0x09);
            writeReg(0x09, val | 0x08);
        }
    }
}

bool spl0601::getCalibParam(void)
{
    uint32_t h;
    uint32_t m;
    uint32_t l;

    h =  readReg(0x10);
    l  =  readReg(0x11);

    this->c0 = (int16_t)h<<4 | l>>4;
    this->c0 = (this->c0&0x0800)? (0xF000|this->c0) : this->c0;

    h =  readReg(0x11);
    l  =  readReg(0x12);
    this->c1 = (int16_t)(h&0x0F)<<8 | l;
    this->c1 = (this->c1&0x0800)? (0xF000|this->c1) : this->c1;

    h =  readReg(0x13);
    m =  readReg(0x14);
    l =  readReg(0x15);
    this->c00 = (int32_t)h<<12 | (int32_t)m<<4 | (int32_t)l>>4;
    this->c00 = (this->c00&0x080000)? (0xFFF00000|this->c00) : this->c00;

    h =  readReg(0x15);
    m =  readReg(0x16);
    l =  readReg(0x17);
    this->c10 = (int32_t)h<<16 | (int32_t)m<<8 | l;
    this->c10 = (this->c10&0x080000)? (0xFFF00000|this->c10) : this->c10;

    h =  readReg(0x18);
    l  =  readReg(0x19);
    this->c01 = (int16_t)h<<8 | l;

    h =  readReg(0x1A);
    l  =  readReg(0x1B);
    this->c11 = (int16_t)h<<8 | l;

    h =  readReg(0x1C);
    l  =  readReg(0x1D);
    this->c20 = (int16_t)h<<8 | l;

    h =  readReg(0x1E);
    l  =  readReg(0x1F);
    this->c21 = (int16_t)h<<8 | l;

    h =  readReg(0x20);
    l  =  readReg(0x21);
    this->c30 = (int16_t)h<<8 | l;

    return true;
}
